#include <cstdlib>
#include <iostream>
#include <libobsensor/hpp/Error.hpp>
#include <memory>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
 
#include "camera.h"

int main(void) {
    try {
        Camera camera(true, false, false);
        camera.start();
        std::string imgname;
        int f = 1;
        while (cv::waitKey(1) != 27) { // ESC的ASCII码是27，按ESC键可以终止程序
            auto frame_set = camera.get();
            if (frame_set != nullptr) {
                cv::Mat img = camera.frame2mat(frame_set->colorFrame());
                if (!img.empty()) {
                    cv::imshow("Color", img);
                    char key = cv::waitKey(1);
                    if (key == 27) break;
                    if (key == 'q' || key == 'Q'){
                        imgname = "../images/" + std::to_string(f++) + ".jpg";
                        cv::imwrite(imgname, img);
                    }
                }
                img = camera.frame2mat(frame_set->irFrame());
                if (!img.empty()) {
                    cv::imshow("IR", img);
                }

                auto depthFrame = frame_set->depthFrame();
                img = camera.frame2mat(depthFrame);
                if (!img.empty()) {
                    cv::imshow("Depth", img);
                }
            }
        }
        camera.stop();
        cv::destroyAllWindows();
    } catch (const ob::Error &e) {
        std::cerr << "Function:" << e.getName() << "\nargs:" << e.getArgs()
                  << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType()
                  << std::endl;
        exit(EXIT_FAILURE);
    } catch (const std::exception &e) {
        std::cerr << e.what() << std::endl;
        exit(EXIT_FAILURE);
    } catch (...) {
        std::cerr << "Unexpected Error!" << std::endl;
        exit(EXIT_FAILURE);
    }
    return EXIT_SUCCESS;
}